﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using grounding.nao;
using grounding.parser;
using grounding.parser.Parsers;
using grounding.grounder;
using grounding.nao.sensors;
using grounding.nao.enums;

namespace grounding.tests.FinalDemo
{
    public enum ProgramState
    {
        None = 0,
        Stage1,
        Stage2,
        Stage3,
        Stage4,
        ActionDemo
    }

    class Program
    {
        static Nao nao = null;
        static NaiveBayesParser nbp = null;
        static SpeechToText stt = null;
        static SVMGrounder grounder = null;
        static ProgramState state = 0;
        
        //globals for the actionDemo function
        static DemoActionParser parser = null;
        static List<string> needsClarification = new List<string>();
        static bool hasAskedForClarification = false;
        static DemoActionTemplate template = null;
       

        static void Main(string[] args)
        {
            nao = null;//new Nao("128.208.4.14", 9559, false);
            Console.WriteLine("Grounding Demo for Nao");
            Console.WriteLine("Type help for more information");
            Console.Write("> ");

            string input;
            while ((input = Console.ReadLine()) != "exit")
            {
                switch (input.Split()[0])
                {
                    case "help":
                        Console.WriteLine("Commands currently understood are: ");
                        Console.WriteLine("help");
                        Console.WriteLine("exit");
                        Console.WriteLine("stage1");
                        Console.WriteLine("stage2");
                        Console.WriteLine("stage3");
                        Console.WriteLine("stage4");
                        Console.WriteLine("status");
                        Console.WriteLine("ls");
                        Console.WriteLine("lots more!");
                        break;
                    case "stage1":
                        stage1(input.Split().Skip(1).ToArray());
                        break;
                    case "stage2":
                        stage2(input.Split().Skip(1).ToArray());
                        break;
                    case "stage3":
                        stage3(input.Split().Skip(1).ToArray());
                        break;
                    case "stage4":
                        stage4(input.Split().Skip(1).ToArray());
                        break;
                    case "stop":
                        stop(input.Split().Skip(1).ToArray());
                        break;
                    case "trainNBP":
                        trainNBP(input.Split().Skip(1).ToArray());
                        break;
                    case "trainGrounder":
                        trainGrounder(input.Split().Skip(1).ToArray());
                        break;
                    case "connect":
                        connect(input.Split().Skip(1).ToArray());
                        break;
                    case "disconnect":
                        disconnect(input.Split().Skip(1).ToArray());
                        break;
                    case "classifyFrame":
                        classifyFrame(input.Split().Skip(1).ToArray());
                        break;
                    case "naoStiffness":
                        break;
                    case "say":
                        say(input.Split().Skip(1).ToArray());
                        break;
                    case "naoAction":
                        naoAction(input.Split().Skip(1).ToArray());
                        break;
                    case "naoCommand":
                        naoCommand(input.Split().Skip(1).ToArray());
                        break;
                    case "status":
                        status(input.Split().Skip(1).ToArray());
                        break;
                    case "ls":
                        ls(input.Split().Skip(1).ToArray());
                        break;
                    default:
                        Console.WriteLine("Unrecognized command: {0}.  Type help for more information.", input);
                        break;
                }

                Console.Write("> ");
            }

            if (nao != null && nao.IsConnected)
                nao.Disconnect();
        }

        #region Command Processing Functions

        private static void stage1(string[] args)
        {
            // Set stage to 1
            state = ProgramState.Stage1;

            // Train Naive Bayes classifier
            trainNBP(new string[] { });

            // Train grounding SVMs
            List<TrainingData> data = new List<TrainingData>();
            //data.Add(TrainingData.LoadFromDisk("DemoDataDistance"));
            // more here
            grounder = new SVMGrounder(1.2, 1.0);
            grounder.Train(data);

            //grounder.Test(nao.GetSensorFrame().ToArray())

            // Finished with stage 1
            state = ProgramState.None;
        }

        private static void stage2(string[] args)
        {
            Console.WriteLine("Stage 2 Learning starting...");
            Console.WriteLine("Type stop and press enter to exit Stage 2");

            // Set stage to 2
            state = ProgramState.Stage2;

            // Make sure we're connected to the Nao robot
            if (nao == null || !nao.IsConnected)
            {
                Console.WriteLine("You need to be connected to a NAO before starting stage 2");
                return;
            }

            // Start Speech-To-Text
            stt = new SpeechToText();
            stt.DebugMode = false;
            stt.SpeechRecognized += new EventHandler<System.Speech.Recognition.SpeechRecognizedEventArgs>(stt_Stage2);
            Console.WriteLine("Speech-To-Text starting up...");
            stt.Start();
        }

        private static void stage3(string[] args)
        {
            Console.WriteLine("Stage 3 Testing starting...");
            Console.WriteLine("Type stop and pres enter to exit Stage 3");
            
            // Set stage to 3
            state = ProgramState.Stage3;

            // Make sure we're connected to the Nao robot
            if (!nao.IsConnected)
            {
                Console.WriteLine("You need to be connected to a NAO before starting stage 3");
                return;
            }

            // Start Speech-To-Text
            stt = new SpeechToText();
            stt.DebugMode = false;
            stt.SpeechRecognized += new EventHandler<System.Speech.Recognition.SpeechRecognizedEventArgs>(stt_Stage3);
            Console.WriteLine("Speech-To-Text starting up...");
            stt.Start();
        }

        private static void stage4(string[] args)
        {
        }

        private static void stop(string[] args)
        {
            switch (state)
            {
                case ProgramState.Stage1:
                    // Nothing to do here
                    break;
                case ProgramState.Stage2:
                    Console.WriteLine("Speech-To-Text stopping...");
                    stt.Stop();
                    stt = null;
                    Console.WriteLine("Retraining grounder");
                    grounder.ReTrain(trainingData.Values.ToList());

                    // save all of the training data
                    foreach (Category key in trainingData.Keys)
                    {
                        TrainingData td = TrainingData.LoadFromDisk(key.Element);
                        if (td == null)
                        {
                            // there was no other data, just set td
                            td = trainingData[key];
                        }
                        else
                        {
                            td.Add(trainingData[key]);
                        }
                        td.SaveToDisk();
                    }                    

                    trainingData.Clear();

                    Console.WriteLine("Exited stage 2");
                    break;
                case ProgramState.Stage3:
                    Console.WriteLine("Speech-To-Text stopping...");
                    stt.Stop();
                    stt = null;
                    Console.WriteLine("Exited stage 3");
                    break;
                case ProgramState.Stage4:
                case ProgramState.ActionDemo:
                    Console.WriteLine("Speech-To-Text stopping...");
                    stt.Stop();
                    stt = null;
                    Console.WriteLine("Exited stage 3");
                    break;
                case ProgramState.None:
                    Console.WriteLine("You weren't in a stage...");
                    break;
                default:
                    throw new InvalidOperationException("How did you get to a state that doesn't exist?");
            }

            // Reset stage to 0
            state = ProgramState.None;
        }

        private static void trainNBP(string[] args)
        {
            string trainingDir = "../../../BayesTrainingFiles";
            if (args.Length > 0)
                trainingDir = args[0];
            Console.WriteLine("Training NB classifier using files located in: {0}", Path.GetFullPath(trainingDir));
            nbp = new NaiveBayesParser(trainingDir);
        }

        private static void trainGrounder(string[] args)
        {
            string trainingFile = "distance_1";
            if (args.Length > 0)
                trainingFile = args[0];
            Console.WriteLine("Training grounders using file: {0}", trainingFile);
            TrainingData newData = TrainingData.LoadFromDisk(trainingFile);
            if (grounder == null)
                grounder = new SVMGrounder(1.2, 1.0);
            grounder.ReTrainSingle(newData);
        }

        private static void connect(string[] args)
        {
            Dictionary<Sensor, Func<IEnumerable<double>, IEnumerable<double>>> transformers = new Dictionary<Sensor, Func<IEnumerable<double>, IEnumerable<double>>>()
            {
                {Sensor.JointCurrentSensor, x => new double[] { x.Average() } },
                {Sensor.FootSensor, x => new double[] { x.Average() } },
                {Sensor.TemperatureSensor, x => new double[] { x.Average() } }
            };
            Sensor[] activeSensors = new Sensor[] 
            {
                Sensor.FootSensor,
                Sensor.Sonar
            };
            if (args.Length != 1)
            {
                nao = new Nao("128.208.4.19", 9559, transformers: transformers,
                    active: activeSensors);
                return;
            }

            try
            {
                if (nao == null)
                {
                    nao = new Nao("128.208.4.19", 9559, transformers: transformers, active: activeSensors);
                }
                else
                {
                    nao.naoIP = args[0];
                    nao.naoPort = 9559;
                    nao.Connect();
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine("Failed to connect to Nao with reason: {0}", ex.Message);
            }
        }

        private static void disconnect(string[] args)
        {
            if(nao != null)
                nao.Disconnect();
        }

        private static void classifyFrame(string[] args)
        {
            if (grounder == null)
            {
                Console.WriteLine("Grounder must be trained first before trying to classify sensor frames");
                return;
            }

            if (nao == null || !nao.IsConnected)
            {
                Console.WriteLine("Must be connected to the nao before trying to classify sensor frames");
                return;
            }

            Dictionary<ILogicElement, double> results = grounder.Test(nao.GetSensorFrame().ToArray());
            Console.WriteLine("Classified as:");
            foreach (ILogicElement category in results.Keys)
                Console.WriteLine("{0,15} : {1}", category.Element, results[category]);
        }

        private static void naoCommand(string[] args)
        {
            Console.WriteLine("NaoCommand stage starting...");
            Console.WriteLine("Type stop and press enter to exit");

            // Set stage to 3
            state = ProgramState.ActionDemo;

            // Make sure we're connected to the Nao robot
            if (!nao.IsConnected)
            {
                Console.WriteLine("You need to be connected to a NAO before starting stage 3");
                return;
            }

            parser = new DemoActionParser();
            // Start Speech-To-Text
            stt = new SpeechToText();
            stt.DebugMode = false;
            stt.SpeechRecognized += new EventHandler<System.Speech.Recognition.SpeechRecognizedEventArgs>(stt_naoCommand);
            Console.WriteLine("Speech-To-Text starting up...");
            stt.Start();
        }

        private static void naoAction(string[] args)
        {
            // Make sure we're connected to the Nao robot
            if (!nao.IsConnected)
            {
                Console.WriteLine("You need to be connected to a NAO before starting stage 3");
                return;
            }
            if (args.Length == 0)
            {
                Console.WriteLine("You must pass something to the action, such as \"arms\", \"grab\" or \"ungrab\"");
            }
            switch (args[0])
            {
                case "arms":
                    nao.ExtendArms();
                    break;
                case "grab":
                    nao.Grab();
                    break;
                case "ungrab":
                    nao.UnGrab();
                    break;
                default:
                    Console.WriteLine("invalid action");
                    break;
            }

            return;

            foreach (string actionName in args)
            {
                try
                {
                    nao.PerformGesture(actionName);
                }
                catch (Exception)
                {
                    Console.WriteLine("Unable to perform action: {0}", actionName);
                }
            }
        }

        private static void say(string[] args)
        {
            if (nao == null || !nao.IsConnected)
            {
                Console.WriteLine("Must be connected to the nao before sending it say commands");
                return;
            }

            nao.Say(string.Join(" ", args));
        }

        private static void status(string[] args)
        {
            Console.WriteLine("Currently in stage {0}", state.ToString());
            Console.WriteLine("Nao is {0}connected", ((nao != null && nao.IsConnected) ? "" : "not "));
            Console.WriteLine("Naive Bayes Parser is {0}trained", ((nbp != null) ? "" : "not "));
            Console.WriteLine("Grounder is {0}trained", ((grounder != null) ? "" : "not "));
            Console.WriteLine("Speech-To-Text is currently {0}running", ((stt != null && stt.IsRunning) ? "" : "not "));
        }

        private static void ls(string[] args)
        {
            if (args.Length == 0)
                args = new string[] { "." };
            foreach (string arg in args)
            {
                Console.WriteLine("{0} : ", arg);
                foreach (string file in Directory.GetFiles(arg))
                    Console.WriteLine(file);
            }
        }

        #endregion

        static Dictionary<Category, TrainingData> trainingData = new Dictionary<Category,TrainingData>();

        static void stt_Stage2(object sender, System.Speech.Recognition.SpeechRecognizedEventArgs e)
        {
            string input = e.Result.Text;
            Console.WriteLine("You said: {0}", e.Result.Text);
            Label classification = (Label)nbp.Parse(e.Result.Text);
            // assume classification works
            //stt.Pause();

            // should print out the individual label (ie, "near|far" not "distance"
            nao.Say(string.Format("\rYou said I am {0}", classification.Element));
            Console.WriteLine("\rParsed input as of category: {0}", classification.Element);
            Console.Write("> ");

            nao.Say("Okay, I got it.");

            // if our training data already contains the parent category, add
            // the current sensor data as an example
            if (trainingData.ContainsKey(classification.Parent))
            {
                // Polls 10 times at 200ms intervals

                List<ISensorFrame> sensorFrames = nao.GetSensorFrames(10, 200);
                foreach (ISensorFrame frame in sensorFrames)
                {
                    trainingData[classification.Parent].AddExample(frame, classification);
                }
            }
            else
            {
                // this part to fix
                TrainingData newData = new TrainingData(classification.Parent.Element,
                    classification.Parent.Identifier, new double[][] { }, new int[] { });
                List<ISensorFrame> sensorFrames = nao.GetSensorFrames(10, 200);
                foreach (ISensorFrame frame in sensorFrames)
                {
                    newData.AddExample(frame, classification);
                }
                trainingData.Add(classification.Parent, newData);
            }
            //stt.Start();
        }

        static void stt_Stage3(object sender, System.Speech.Recognition.SpeechRecognizedEventArgs e)
        {
            Console.WriteLine("You said: {0}", e.Result.Text);
            Label classification = (Label)nbp.Parse(e.Result.Text);
            Category category = classification.Parent;

            Console.WriteLine("You asked me about the category {0}", category.Element);
            nao.Say(string.Format("You asked me about the category {0}", category.Element));

            Dictionary<ILogicElement, double> result = grounder.Test(nao.GetSensorFrame().ToArray());

            Console.WriteLine("I am {0}", (result.ContainsKey(category.Positive) ? category.Positive.Element : category.Negative.Element));
            nao.Say(string.Format("I am {0}", (result.ContainsKey(category.Positive) ?
                category.Positive.PrettyDescription : category.Negative.PrettyDescription)));
        }

        static void stt_naoCommand(object sender, System.Speech.Recognition.SpeechRecognizedEventArgs e)
        {
            Console.WriteLine("I heard you say: {0}", e.Result.Text);

            if (needsClarification.Count == 0)
            {
                template = (DemoActionTemplate)parser.Parse(e.Result.Text);
                if (template == null)
                    Console.WriteLine("Ignored phrase");
                else
                {
                    Console.WriteLine("I understood it as:" + template.Type);

                    foreach (string option in template.info.Keys)
                    {
                        if (template.info[option] == null)
                            needsClarification.Add(option);
                        Console.WriteLine("\t" + option + ": " + template.info[option]);
                    }

                    if (needsClarification.Count == 0)
                    {
                        makeMove(template);
                        hasAskedForClarification = false;
                    }
                    else
                    {
                        foreach (string option in needsClarification)
                        {
                            if (!hasAskedForClarification)
                                nao.Say("Please clarify " + option);
                            hasAskedForClarification = true;
                            Console.WriteLine("You need to clarify " + option);
                        }
                    }
                }
            }
            else
            {
                template.FillTemplate(e.Result.Text, true);
                needsClarification.Clear();
                foreach (string option in template.info.Keys)
                {
                    if (template.info[option] == null)
                        needsClarification.Add(option);
                }

                if (needsClarification.Count == 0)
                {
                    makeMove(template);
                    hasAskedForClarification = false;
                }
                else
                {
                    foreach (string option in needsClarification)
                    {
                        if (!hasAskedForClarification)
                            nao.Say("Please clarify " + option);
                        hasAskedForClarification = true;
                        Console.WriteLine("You need to clarify " + option);
                    }
                }
            }
        }

        static void makeMove(DemoActionTemplate fullTemplate)
        {
            nao.StopTalking();
            if (fullTemplate.Type == "walking")
            {
                
                float distance;
                Direction direction;
                try
                {
                    //distance = float.Parse(fullTemplate.info["distance"]);
                    switch (fullTemplate.info["distance"])
                    {
                        case "one":
                            distance = 1;
                            break;
                        case "two":
                            distance = 2;
                            break;
                        case "three":
                            distance = 3;
                            break;
                        default:
                            throw new ArgumentException("YOU DIDN'T PUT A NUMBER");
                    }
                    direction = (Direction)Enum.Parse(typeof(Direction), fullTemplate.info["direction"], true);
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Failed to parse filled in template values");
                    Console.WriteLine(ex.StackTrace);
                    return;
                }

                nao.Walk(distance / 5, direction);
            }
            else if (fullTemplate.Type == "raiseArms")
            {
                switch (fullTemplate.info["arm"])
                {
                    case "left":
                        nao.ExtendLeftArm();
                        break;
                    case "right":
                        nao.ExtendRightArm();
                        break;
                    case "both":
                        nao.ExtendArms();
                        break;
                }
            }
            else if (fullTemplate.Type == "closeHands")
            {
                switch (fullTemplate.info["hand"])
                {
                    case "left":
                        nao.GrabLeft();
                        break;
                    case "right":
                        nao.GrabRight();
                        break;
                    case "both":
                        nao.Grab();
                        break;
                }
            }
            else if (fullTemplate.Type == "openHands")
            {
                switch (fullTemplate.info["hand"])
                {
                    case "left":
                        nao.UnGrabLeft();
                        break;
                    case "right":
                        nao.UnGrabRight();
                        break;
                    case "both":
                        nao.UnGrab();
                        break;
                }
            }
            else if (fullTemplate.Type == "stiff")
                nao.StiffAll();
            else if (fullTemplate.Type == "unstiff")
                nao.UnstiffAll();
        }
    }
}
